#include "qbcimath.h"
#include "sigpack.h"

bci::QBciArmaSignal bci::BciMathTool::sqrtmat(const bci::QBciArmaSignal &M) {
  ArmaVec eigval;
  ArmaMat eigvec;

  arma::eig_sym(eigval, eigvec, M.d->mat);

  ArmaMat dig(eigvec.n_rows, eigvec.n_cols, arma::fill::zeros);
  for (arma::uword iD = 0; iD < dig.n_cols; ++iD) {
    dig(iD, iD) = eigval(iD);
  }

  return QBciArmaSignal(eigvec * arma::sqrt(dig) * arma::inv(eigvec));
}

double bci::BciMathTool::cca(const bci::QBciArmaSignal &x,
                             const bci::QBciArmaSignal &y) {
  ArmaMat x_norm =
      (x.d->mat - repmat(arma::mean(x.d->mat), x.d->mat.n_rows, 1)) /
      repmat(arma::stddev(x.d->mat), x.d->mat.n_rows, 1);
  ArmaMat y_norm =
      (y.d->mat - repmat(arma::mean(y.d->mat), y.d->mat.n_rows, 1)) /
      repmat(arma::stddev(y.d->mat), y.d->mat.n_rows, 1);

  ArmaMat xx_cov = arma::cov(x.d->mat);
  ArmaMat yy_cov = arma::cov(y.d->mat);
  ArmaMat xy_cov = arma::cov(x.d->mat, y.d->mat);

  ArmaMat M = BciMathTool::sqrtmat(xx_cov).d->mat.i() * xy_cov *
              BciMathTool::sqrtmat(yy_cov).d->mat.i();

  return arma::svd(M).max();
}

bci::QBciArmaSignal bci::BciMathTool::filt50(const bci::QBciArmaSignal &sig) {
  sp::FIR_filt<double, double, double> Filter;
  arma::vec filterNum({
      0,
      -9.20096451165986e-09,
      -5.668363067145799e-07,
      -1.766452229965693e-06,
      -2.588842010280603e-06,
      -1.378892659518007e-06,
      3.002946197570701e-06,
      9.906277050345915e-06,
      1.623648333891303e-05,
      1.700087246231291e-05,
      7.142317287604613e-06,
      -1.589576450110338e-05,
      -4.994498386556096e-05,
      -8.742230799466683e-05,
      -0.0001169621761813544,
      -0.0001272944892104421,
      -0.0001122607351658798,
      -7.507466497304677e-05,
      -2.978457805219759e-05,
      1.419877665289191e-06,
      -4.997173559844206e-06,
      -6.477875326473411e-05,
      -0.0001775122906626734,
      -0.0003225618876659452,
      -0.0004618920042840506,
      -0.0005502355510824914,
      -0.0005502947903992199,
      -0.0004482347374984786,
      -0.0002636421124192615,
      -4.905319421613869e-05,
      0.0001228739428665171,
      0.0001822026765562181,
      8.805275035836301e-05,
      -0.0001506142137456519,
      -0.0004684142070832215,
      -0.0007561680264531165,
      -0.0008916064833292231,
      -0.0007802492313481218,
      -0.0003935121981073789,
      0.0002100818956361507,
      0.0008909804035524746,
      0.001464272798890435,
      0.001757068944290618,
      0.001670724246526399,
      0.001226711998583747,
      0.000577481399300012,
      -2.682649817139947e-05,
      -0.0003107653608663412,
      -6.558017238711076e-05,
      0.0007683550406674708,
      0.002058649797587232,
      0.003498005262169172,
      0.004683773553357546,
      0.005241134512744989,
      0.004953442308376656,
      0.00385595811461033,
      0.00225600323130509,
      0.0006629985142672215,
      -0.000359417513472079,
      -0.000378817833196217,
      0.0007413122830592218,
      0.002756114607592585,
      0.005071997437307539,
      0.006901000632491426,
      0.007499809384998645,
      0.006423314306831073,
      0.003709318773602095,
      -7.58148378529647e-05,
      -0.003962510737218886,
      -0.00685286160954258,
      -0.007871231700512286,
      -0.006682988434684305,
      -0.003671850794313817,
      9.95750496662451e-05,
      0.003158722714023971,
      0.004060140349680967,
      0.001867251165592091,
      -0.003473469984539369,
      -0.01100074903408857,
      -0.01892113005140053,
      -0.02508494892738716,
      -0.02763825188791161,
      -0.02566363969145516,
      -0.01960487395502723,
      -0.01131421137569616,
      -0.003660205204064614,
      0.0002381089167034492,
      -0.002036383016622541,
      -0.01132356660904867,
      -0.02635685151866082,
      -0.04377525852292004,
      -0.05873487830104894,
      -0.06601224022238945,
      -0.06134428755060816,
      -0.0426630803155483,
      -0.010886435671368,
      0.02997736603663821,
      0.07348428715118654,
      0.1120457652765745,
      0.1385622182296706,
      0.1480058303428571,
      0.1385622182296706,
      0.1120457652765745,
      0.07348428715118654,
      0.02997736603663821,
      -0.010886435671368,
      -0.0426630803155483,
      -0.06134428755060816,
      -0.06601224022238945,
      -0.05873487830104894,
      -0.04377525852292004,
      -0.02635685151866082,
      -0.01132356660904867,
      -0.002036383016622541,
      0.0002381089167034492,
      -0.003660205204064614,
      -0.01131421137569616,
      -0.01960487395502723,
      -0.02566363969145516,
      -0.02763825188791161,
      -0.02508494892738716,
      -0.01892113005140053,
      -0.01100074903408857,
      -0.003473469984539369,
      0.001867251165592091,
      0.004060140349680967,
      0.003158722714023971,
      9.95750496662451e-05,
      -0.003671850794313817,
      -0.006682988434684305,
      -0.007871231700512286,
      -0.00685286160954258,
      -0.003962510737218886,
      -7.58148378529647e-05,
      0.003709318773602095,
      0.006423314306831073,
      0.007499809384998645,
      0.006901000632491426,
      0.005071997437307539,
      0.002756114607592585,
      0.0007413122830592218,
      -0.000378817833196217,
      -0.000359417513472079,
      0.0006629985142672215,
      0.00225600323130509,
      0.00385595811461033,
      0.004953442308376656,
      0.005241134512744989,
      0.004683773553357546,
      0.003498005262169172,
      0.002058649797587232,
      0.0007683550406674708,
      -6.558017238711076e-05,
      -0.0003107653608663412,
      -2.682649817139947e-05,
      0.000577481399300012,
      0.001226711998583747,
      0.001670724246526399,
      0.001757068944290618,
      0.001464272798890435,
      0.0008909804035524746,
      0.0002100818956361507,
      -0.0003935121981073789,
      -0.0007802492313481218,
      -0.0008916064833292231,
      -0.0007561680264531165,
      -0.0004684142070832215,
      -0.0001506142137456519,
      8.805275035836301e-05,
      0.0001822026765562181,
      0.0001228739428665171,
      -4.905319421613869e-05,
      -0.0002636421124192615,
      -0.0004482347374984786,
      -0.0005502947903992199,
      -0.0005502355510824914,
      -0.0004618920042840506,
      -0.0003225618876659452,
      -0.0001775122906626734,
      -6.477875326473411e-05,
      -4.997173559844206e-06,
      1.419877665289191e-06,
      -2.978457805219759e-05,
      -7.507466497304677e-05,
      -0.0001122607351658798,
      -0.0001272944892104421,
      -0.0001169621761813544,
      -8.742230799466683e-05,
      -4.994498386556096e-05,
      -1.589576450110338e-05,
      7.142317287604613e-06,
      1.700087246231291e-05,
      1.623648333891303e-05,
      9.906277050345915e-06,
      3.002946197570701e-06,
      -1.378892659518007e-06,
      -2.588842010280603e-06,
      -1.766452229965693e-06,
      -5.668363067145799e-07,
      -9.20096451165986e-09,
      0,
  });
  Filter.set_coeffs(filterNum);

  ArmaMat mat;
  for (int iC = 0; iC < sig.n_channels(); ++iC) {
    mat = arma::join_rows(mat, Filter.filter(ArmaCol(sig.d->mat.col(iC))));
  }
  return QBciArmaSignal(mat);
}
